#!/usr/bin/env python
# Author: iris and jeff

PKG = 'sound_localization'
NAME = 'sample_collector'

import roslib; roslib.load_manifest(PKG)
import rospy

from sound_localization.msg import *
from geometry_msgs.msg import *


class SampleCollector():
#make a global variable for the latest location, to update each time noise is made
    def __init__(self):
        
        # Initialize the node
        rospy.init_node(NAME, anonymous=True)

        self.robot_location = Point(x=0.0, y=0.0, z=0.0)

        # Subscribe to time_delay which is of type SoundDelayVector
        rospy.Subscriber("/sound_localization/sound_delay", SoundDelayVector, self.time_delay_callback)

        # Subscribe to robot location
        rospy.Subscriber("/sound_localization/robot_location", Point, self.robot_location_callback)
        
        self.pub = rospy.Publisher('/sound_localization/audio_location_sample', AudioLocationSample)


    def time_delay_callback(self, data):
        try:
            sample = AudioLocationSample()
            sample.robot_location = self.robot_location
            sample.delays_sample = data
            
            # Publish audio-location sample
            self.pub.publish(sample)

        except rospy.ServiceException, e:
            print "Reply failed: %s"%e

    def robot_location_callback(self, data):
        try:
            self.robot_location = data
        except rospy.ServiceException, e:
            print "Reply failed: %s"%e


if __name__ == '__main__':
    try:
        st = SampleCollector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
